#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
//#include <moveit/planning_scene_interface/planning_scene_interface.h>

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)//引用 & 作函数参数，形参可以修饰实参
{
    int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();//获取关节个数
    
    for(int i=0; i<plan.trajectory_.joint_trajectory.points.size(); i++)//通过for循环对plan中所有的轨迹点作一个遍历
    {
        plan.trajectory_.joint_trajectory.points[i].time_from_start *= 1/scale;//速度变慢，时间变长为 1/scale 倍
        
        for(int j=0; j<n_joints; j++)//遍历各个关节，每一个关节的速度和加速度数据都要作一个尺度的变化
        {
            plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;//速度变化为原来的 scale
            plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale*scale;//加速度变化为原来的 scale * scale
        }
    }
}



int main(int argc, char **argv)
{
  ros::init(argc, argv, "moveit_custom_demo");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface group("jr603arm");
  
  group.setPlanningTime(100.0);
  group.setNumPlanningAttempts(50);
  // 设置机器人终端的目标位置
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.499998;
  target_pose1.orientation.x= -0.500019;
  target_pose1.orientation.y = -0.499959;
  target_pose1.orientation.z = 0.500024;

  target_pose1.position.x = -0.168002;
  target_pose1.position.y = 0.000004;
  target_pose1.position.z = 0.243004;
  /*
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 0.536073;
  target_pose1.orientation.x= 0.41399;
  target_pose1.orientation.y = -0.529246;
  target_pose1.orientation.z = 0.511015;

  target_pose1.position.x = -0.0773;
  target_pose1.position.y = -0.13191;
  target_pose1.position.z = 0.187051;
  */



  // 设置规划轨迹的精度和采样频率
  //group.setInterpolationMethod(moveit::planning_interface::CartesianInterpolator);  // 选择插值方法
  group.setMaxVelocityScalingFactor(0.1);
  group.setMaxVelocityScalingFactor(0.8);
  group.setPoseTarget(target_pose1);
  group.setGoalTolerance(0.001);

  // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  moveit::core::MoveItErrorCode success = group.plan(my_plan);
  

  
  ROS_INFO("Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
  //ROS_INFO_STREAM("Planning failed with error code: " << moveit::core::MoveItErrorCode(success).toString());
 // ROS_INFO_STREAM("Planning failed with error code: " << moveit::core::MoveItErrorCode(success).getVal() << " (" << moveit::core::MoveItErrorCode(success).asStr() << ")");

  scale_trajectory_speed(my_plan, 0.1);
  // 访问规划的轨迹
  const moveit_msgs::RobotTrajectory& trajectory = my_plan.trajectory_;

  // 打印轨迹点
  for (size_t i = 0; i < trajectory.joint_trajectory.points.size(); ++i)
  {
    const trajectory_msgs::JointTrajectoryPoint& point = trajectory.joint_trajectory.points[i];
    ROS_INFO("Waypoint %zu:", i);
    for (size_t j = 0; j < point.positions.size(); ++j)
    {
    	if(i>0)
    	{
    		ROS_INFO("Vel %zu: %f", j, point.positions[j]-trajectory.joint_trajectory.points[i-1].positions[j]);
    	}
      ROS_INFO("Joint %zu: %f", j, point.positions[j]);
    }
  }
  
  // 让机械臂按照规划的轨迹开始运动。
  if (success)
    group.execute(my_plan);

  robot_state::RobotStatePtr current_state = group.getCurrentState();
  // 获取机器人当前末端的位姿
  const Eigen::Isometry3d& end_effector_pose = current_state->getGlobalLinkTransform("link6");
  // 获取位姿的旋转部分（四元数）
  Eigen::Quaterniond quaternion(end_effector_pose.rotation());
  // 打印初始状态的xyzw值
  ROS_INFO("Initial Pose (xyzw): %.3f %.3f %.3f %.3f",
           quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());


  ros::shutdown(); 
  return 0;
}

